/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, 2013, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*         David V. Lu!!
*********************************************************************/
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <tf2_ros/message_filter.h>

#include <pluginlib/class_list_macros.hpp>
#include <sensor_msgs/point_cloud2_iterator.h>

// 障碍层插件和名称空间声明:
PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)

using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;

namespace costmap_2d
{

void ObstacleLayer::onInitialize()
{
    // 创建多个节点句柄nh("~/" + name_)、g_nh、prefix_nh和source_node(nh, source)，
    // 通过方法param(param_name, param_val, default_val)从参数服务器检索指示的值param_name,
    // 若成功，则将结果存储在param_val中，否则将default_val赋值给param_val。
    ros::NodeHandle nh("~/" + name_), g_nh;
    rolling_window_ = layered_costmap_->isRolling();
    bool track_unknown_space;
    nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
#ifdef SEMANTIC
    int inflation_option_temp;
    nh.param("inflation_option", inflation_option_temp, static_cast<int>(costmap_2d::XJU_OPTION_NORM));
    inflation_option_ = static_cast<uint8_t>(inflation_option_temp);
    if (track_unknown_space) setDefaultValue(toOri(XJU_COST_NO_INFORMATION, inflation_option_));
    else setDefaultValue(toOri(XJU_COST_FREE_SPACE, inflation_option_));
#else
    if (track_unknown_space) default_value_ = NO_INFORMATION;
    else default_value_ = FREE_SPACE;
#endif
    // 设置障碍物层的代价值、x值、y值、分辨率和原点坐标
    ObstacleLayer::matchSize();
    current_ = true;
    global_frame_ = layered_costmap_->getGlobalFrameID();
    double transform_tolerance; // 发布变换的时间，以指示此变换在未来有效
    nh.param("transform_tolerance", transform_tolerance, 0.2);
    // 获取观测源，例如 observation_sources: laser_scan_sensor
    // laser_scan_sensor: {sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking: true, clearing: true}
    std::string topics_string;
    // get the topics that we'll subscribe to from the parameter server
    nh.param("observation_sources", topics_string, std::string(""));
    ROS_INFO("Subscribed to Topics: %s", topics_string.c_str());
    // now we need to split the topics based on whitespace which we can use a stringstream for
    std::stringstream ss(topics_string);
    std::string source;
    while (ss >> source)
    {
        ros::NodeHandle source_node(nh, source);
        // get the parameters for the specific topic
        double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
        std::string topic, sensor_frame, data_type;
        bool inf_is_valid, clearing, marking;
        /* base_scan: {
              topic: topic_name,
              sensor_frame: frame_name,
              observation_persistence: 0.0,  # 观测保持时间 默认值0是只保留最新的 单位: s
              expected_update_rate: 0,       # 更新频率 默认值0是没有限制
              data_type: LaserScan,          # 数据类型仅支持[PointCloud2/PointCloud/LaserScan]三种类型
              min_obstacle_height: 0.08,     # 障碍物最小高度 单位: m
              max_obstacle_height: 0.4,      # 障碍物最大高度 单位: m
              inf_is_valid: false,           # Inf是否有效
              clearing: true,                # 是否会清除
              marking: true,                 # 是否会标记
              obstacle_range: 2.5,           # 传感器探测到障碍物的最大范围值   单位: m
              raytrace_range: 3.0            # 传感器感知到周围环境的最大范围值 单位: m
            }
         */
        source_node.param("topic", topic, source);
        source_node.param("sensor_frame", sensor_frame, std::string(""));
        source_node.param("observation_persistence", observation_keep_time, 0.0);
        source_node.param("expected_update_rate", expected_update_rate, 0.0);
        source_node.param("data_type", data_type, std::string("PointCloud"));
        source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
        source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
        source_node.param("inf_is_valid", inf_is_valid, false);
        source_node.param("clearing", clearing, false);
        source_node.param("marking", marking, true);
        if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
        {
            ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
            throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
        }
        std::string raytrace_range_param_name, obstacle_range_param_name;
        double obstacle_range = 2.5; // get the obstacle range for the sensor 确定传感器所能探测到障碍物的最大范围值   单位: m
        double raytrace_range = 3.0; // get the raytrace range for the sensor 确定传感器所能感知到周围环境的最大范围值 单位: m
        if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
        {
            source_node.getParam(obstacle_range_param_name, obstacle_range);
        }
        if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
        {
            source_node.getParam(raytrace_range_param_name, raytrace_range);
        }
        ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s",
                source.c_str(), topic.c_str(), sensor_frame.c_str());
        // 创建观察缓冲区ObservationBuffer(),push_back()为在observation_buffers_这个vector中的最后添加一个元素
        // create an observation buffer
        observation_buffers_.push_back(
                boost::shared_ptr<ObservationBuffer>(
                        new ObservationBuffer(topic, observation_keep_time, expected_update_rate,
                                              min_obstacle_height, max_obstacle_height, obstacle_range,
                                              raytrace_range, *tf_, global_frame_,
                                              sensor_frame, transform_tolerance)));
        // 检查是否将此缓冲区添加到标记观察缓冲区
        // 此时marking_buffers_虽添加了观察区的数据，但还未调用传感器数据回调函数，其值为空值
        // check if we'll add this buffer to our marking observation buffers
        if (marking) marking_buffers_.push_back(observation_buffers_.back());
        // 检查是否还要将此缓冲区添加到清除观察缓冲区
        // check if we'll also add this buffer to our clearing observation buffers
        if (clearing) clearing_buffers_.push_back(observation_buffers_.back());
        ROS_DEBUG("Created an observation buffer for source %s, topic %s, global frame: %s, "
                "expected update rate: %.2f, observation persistence: %.2f",
                source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
        // 为"PointCloud2"、"PointCloud"和"LaserScan"类型的数据，分别调用回调函数，用于传感器数据的处理
        // create a callback for the topic
        if (data_type == "LaserScan")
        {
            boost::shared_ptr<message_filters::Subscriber<sensor_msgs::LaserScan>> sub(
                    new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
            boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan>> filter(
                    new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
            if (inf_is_valid)
            {
                filter->registerCallback(
                        [this, buffer = observation_buffers_.back()](auto &msg) { laserScanValidInfCallback(msg, buffer); });
            }
            else
            {
                filter->registerCallback(
                        [this, buffer = observation_buffers_.back()](auto &msg) { laserScanCallback(msg, buffer); });
            }
            observation_subscribers_.emplace_back(sub);
            observation_notifiers_.emplace_back(filter);
            observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
        }
        else if (data_type == "PointCloud")
        {
            boost::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud>> sub(
                    new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
            if (inf_is_valid)
            {
                ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
            }
            boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::PointCloud>> filter(
                    new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
            filter->registerCallback([this, buffer = observation_buffers_.back()](auto &msg)
                                     { pointCloudCallback(msg, buffer); });
            observation_subscribers_.emplace_back(sub);
            observation_notifiers_.emplace_back(filter);
        }
        else if (data_type == "PointCloud2")
        {
            boost::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> sub(
                    new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
            if (inf_is_valid)
            {
                ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
            }
            boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::PointCloud2>> filter(
                    new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
            filter->registerCallback([this, buffer = observation_buffers_.back()](auto &msg)
                                     { pointCloud2Callback(msg, buffer); });
            observation_subscribers_.emplace_back(sub);
            observation_notifiers_.emplace_back(filter);
        }
        else ROS_ERROR_STREAM("topic: " << topic << " data_type: " << data_type << " is ERROR!!!");
        if (!sensor_frame.empty())
        {
            std::vector<std::string> target_frames;
            target_frames.push_back(global_frame_);
            target_frames.push_back(sensor_frame);
            observation_notifiers_.back()->setTargetFrames(target_frames);
        }
    }
    dsrv_ = NULL;
    setupDynamicReconfigure(nh);
}

void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle &nh)
{
    dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
    dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb =
            [this](auto &config, auto level) { reconfigureCB(config, level); };
    dsrv_->setCallback(cb);
}

ObstacleLayer::~ObstacleLayer() { delete dsrv_; }

void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
{
    enabled_ = config.enabled;
    footprint_clearing_enabled_ = config.footprint_clearing_enabled;
    max_obstacle_height_ = config.max_obstacle_height;
    combination_method_ = config.combination_method;
}
// 激光回调函数
void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr &message,
                                      const boost::shared_ptr<ObservationBuffer> &buffer)
{
    // Step1: project the laser into a point cloud 将激光转换成点云
    sensor_msgs::PointCloud2 cloud;
    cloud.header = message->header;
    // Step2: project the scan into a point cloud 将sensor_msgs::LaserScan转换为目标帧中的sensor_msgs::PointCloud2
    try
    {
        projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
    }
    catch (tf2::TransformException &ex)
    {
        ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
                 global_frame_.c_str(), ex.what());
        projector_.projectLaser(*message, cloud);
    }
    catch (std::runtime_error &ex)
    {
        ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. "
                 "Ignore this laser scan. what(): %s", ex.what());
        return; // ignore this message
    }
    // Step3: buffer the point cloud 缓冲点云信息
    buffer->lock();
    buffer->bufferCloud(cloud);
    buffer->unlock();
}
// 将正无穷大("Inf")筛选为max_range的激光回调
void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr &raw_message,
                                              const boost::shared_ptr<ObservationBuffer> &buffer)
{
    // Filter positive infinities ("Inf"s) to max_range.
    // Step 1:将正无穷大("Inf")筛选为max_rang
    float epsilon = 0.0001;  // a tenth of a millimeter 0.1毫米
    sensor_msgs::LaserScan message = *raw_message;
    for (size_t i = 0; i < message.ranges.size(); i++)
    {
        float range = message.ranges[i];
        if (!std::isfinite(range) && range > 0)
        {
            message.ranges[i] = message.range_max - epsilon;
        }
    }
    // Step1: project the laser into a point cloud
    sensor_msgs::PointCloud2 cloud;
    cloud.header = message.header;
    // Step2: project the scan into a point cloud
    try
    {
        // projector_是LaserProjection的实例，laserProjection也是ros中的库，不在navigation代码中
        projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
    }
    catch (tf2::TransformException &ex)
    {
        ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
                 global_frame_.c_str(), ex.what());
        projector_.projectLaser(message, cloud);
    }
    catch (std::runtime_error &ex)
    {
        ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. "
                 "Ignore this laser scan. what(): %s", ex.what());
        return; //ignore this message
    }
    // Step3: buffer the point cloud
    buffer->lock();
    buffer->bufferCloud(cloud);
    buffer->unlock();
}

// 处理缓冲点云pointcloud消息的回调函数
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr &message,
                                       const boost::shared_ptr<ObservationBuffer> &buffer)
{
    sensor_msgs::PointCloud2 cloud2;
    // 将sensor_msgs::PointCloud消息转换为sensor_msgs::PointCloud2消息。
    if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2))
    {
        ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
        return;
    }
    // buffer the point cloud
    buffer->lock();
    buffer->bufferCloud(cloud2);
    buffer->unlock();
}

// 处理缓冲点云pointcloud2消息的回调函数
void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr &message,
                                        const boost::shared_ptr<ObservationBuffer> &buffer)
{
    // buffer the point cloud
    buffer->lock();
//    std::deque<geometry_msgs::PoseStamped> UnsyncedRobotPose(layered_costmap_->RobotPoseDeque);
    buffer->bufferCloud(*message);
    buffer->unlock();
}

// 传入参数 robot_x、robot_y和robot_yaw 机器人位姿
// min_y、min_y、max_x和 max_y用于 更新边界轮廓
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x,
                                 double *min_y, double *max_x, double *max_y)
{
    // Step 1:判断是否处于滚动窗口，若是，则进行更新原点坐标
    // updateOrigin() 确定更新边界轮廓的原点坐标
    // getSizeInMetersX() 返回 （size_x_ - 1 + 0.5) * resolution_
    // getSizeInMetersY() 返回 （size_y_ - 1 + 0.5) * resolution_
    if (rolling_window_) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
    // Step 2: 使用多余的边界
    useExtraBounds(min_x, min_y, max_x, max_y);
    bool current = true;
    std::vector<Observation> observations, clearing_observations;
    // Step 3:获取Marking观测值 get the marking observations
    current = current && getMarkingObservations(observations);
    // Step 4:获取Clearing观测值 get the clearing observations
    current = current && getClearingObservations(clearing_observations);
    // Step 5:更新全局当前状态 update the global current status
    current_ = current;
    // Step 6:使用raytrace方法清除障碍物 raytrace freespace 光线跟踪自由空间
    for (const auto & clearing_observation : clearing_observations)
    {
        raytraceFreespace(clearing_observation, min_x, min_y, max_x, max_y);
    }
    // place the new obstacles into a priority queue... each with a priority of zero to begin with
    // Step 7: 将新障碍物放入优先级队列...每个都以零优先级开始遍历
    for (const auto & obs : observations)
    {
        const sensor_msgs::PointCloud2 &cloud = *(obs.cloud_);
        double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
        // 定义若干容器，来存储x,y,z
        sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
        sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
        sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
        // 先遍历x
        for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
        {
            // 声明指针
            double px = *iter_x, py = *iter_y, pz = *iter_z;
            // if the obstacle is too high or too far away from the robot we won't add it
            // 1)如果障碍物太高或离机器人太远，我们不会添加它
            if (pz > max_obstacle_height_)
            {
                ROS_DEBUG("The point is too high");
                continue;
            }
            // compute the squared distance from the hitpoint to the pointcloud's origin
            // 2)计算从击中点到点云原点的平方距离
            double sq_dist =
                    (px - obs.origin_.x) * (px - obs.origin_.x) +
                    (py - obs.origin_.y) * (py - obs.origin_.y) +
                    (pz - obs.origin_.z) * (pz - obs.origin_.z);
            // if the point is far enough away... we won't consider it
            // 3)如果点足够远...我们不会考虑它
            if (sq_dist >= sq_obstacle_range)
            {
                ROS_DEBUG("The point is too far away");
                continue;
            }
            // now we need to compute the map coordinates for the observation
            // 4)现在我们需要计算观测的地图坐标
            unsigned int mx, my;
            if (!worldToMap(px, py, mx, my))
            {
                ROS_DEBUG("Computing map coords failed");
                continue;
            }
            unsigned int index = getIndex(mx, my);
            // 致命障碍物
#ifdef SEMANTIC
            // setXJUcost(costmap_[index], XJU_COST_LETHAL_OBSTACLE);
            setXJUgrid(costmap_[index], XJU_COST_LETHAL_OBSTACLE, inflation_option_);
#else
            costmap_[index] = LETHAL_OBSTACLE;
#endif
            // TODO by tony: in future, if obs point has special semantic,
            //  we can also set option here now option is set by default value
            // 更新参数中指定的边界框以包括位置(x,y)
            touch(px, py, min_x, min_y, max_x, max_y);
        }
    }
    // Step 8:更新机器人的footprint
    updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

// 更新机器人的footprint
void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw,
        double *min_x, double *min_y, double *max_x, double *max_y)
{
    if (!footprint_clearing_enabled_) return;
    transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
    for (auto & i : transformed_footprint_)
    {
        touch(i.x, i.y, min_x, min_y, max_x, max_y);
    }
}

// 更新代价值
void ObstacleLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
{
    if (footprint_clearing_enabled_)
    {
#ifdef SEMANTIC
        setConvexPolygonCost(transformed_footprint_, XJU_COST_FREE_SPACE);
#else
        setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
#endif
    }
    // 更新master_grid的方法
    switch (combination_method_)
    {
        case 0:  // Overwrite
            // 覆盖更新，地图层的每个有效值（不含未知状态（NO_INFORMATION））写进master_grid
            // master_grid：地图层的特定边界矩形的主栅格对象，
            // min_i、min_j: 特定边界矩形的左下角顶点的x、y坐标
            // max_i、max_j: 特定边界矩形的右上角顶点的x、y坐标
            updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
            break;
        case 1:  // Maximum
            // 最大值更新，主栅格的新值取master_grid中各层中的最大值
            // 特殊情况:
            // a) 如果主代价值是未知状态（NO_INFORMATION），则被覆盖更新
            // b) 如果所操作的地图层的代价值是未知状态（NO_INFORMATION），则主代价值不变
            updateWithMax(master_grid, min_i, min_j, max_i, max_j);
            break;
        default:  // Nothing
            break;
    }
}

// 添加静态观察
void ObstacleLayer::addStaticObservation(costmap_2d::Observation &obs, bool marking, bool clearing)
{
    if (marking) static_marking_observations_.push_back(obs);
    if (clearing) static_clearing_observations_.push_back(obs);
}

// 清除静态观察
void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
{
    if (marking) static_marking_observations_.clear();
    if (clearing) static_clearing_observations_.clear();
}

// 获取标记观察
bool ObstacleLayer::getMarkingObservations(std::vector<Observation> &marking_observations) const
{
    bool current = true;
    // get the marking observations
    for (const auto & marking_buffer : marking_buffers_)
    {
        marking_buffer->lock();
        marking_buffer->getObservations(marking_observations);
        current = marking_buffer->isCurrent() && current;
        marking_buffer->unlock();
    }
    marking_observations.insert(marking_observations.end(),
            static_marking_observations_.begin(), static_marking_observations_.end());
    return current;
}

// 获取清除观察，与标记观察是相对的
bool ObstacleLayer::getClearingObservations(std::vector<Observation> &clearing_observations) const
{
    bool current = true;
    // get the clearing observations 获取用于清除的观察
    for (const auto & clearing_buffer : clearing_buffers_)
    {
        clearing_buffer->lock();
        clearing_buffer->getObservations(clearing_observations);
        current = clearing_buffer->isCurrent() && current;
        clearing_buffer->unlock();
    }
    clearing_observations.insert(clearing_observations.end(),
            static_clearing_observations_.begin(), static_clearing_observations_.end());
    return current;
}

void ObstacleLayer::raytraceFreespace(const Observation &clearing_observation, double *min_x, double *min_y,
                                      double *max_x, double *max_y)
{
    // Step 1: 输入clearing_observation,会更新min_*,max_*,获取clearing_observation的原点x,y以及点云
    double ox = clearing_observation.origin_.x;
    double oy = clearing_observation.origin_.y;
    const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
    // Step 2: get the map coordinates of the origin of the sensor 获取地图坐标系下传感器原点
    unsigned int x0, y0;
    if (!worldToMap(ox, oy, x0, y0))
    {
        ROS_WARN_THROTTLE(1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. "
                "So, the costmap cannot raytrace for it.", ox, oy);
        return;
    }
    // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
    // Step 3: 我们可以在内循环之外预先计算地图的点...我们稍后将需要这些 存储origin_*,map_end_*
    double origin_x = origin_x_, origin_y = origin_y_;
    double map_end_x = origin_x + size_x_ * resolution_;
    double map_end_y = origin_y + size_y_ * resolution_;
    // 更新参数中指定的边界框，以包含位置(x,y)
    touch(ox, oy, min_x, min_y, max_x, max_y);
    // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
    // Step 4: 对于云中的每个点，我们希望从原点开始追踪一条线，并清除沿其的障碍物
    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
    // Step 5: 从迭代器iter_x开始遍历 这里获取连线时，要注意该点是否超出地图范围，如果超出，则通过相似三角形去除连线在地图外的部分。
    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
    {
        double wx = *iter_x;
        double wy = *iter_y;
        // now we also need to make sure that the enpoint we're raytracing to isn't off the costmap and scale if necessary
        // 1)确保要进行射线扫描的点不会偏离costmap(如有必要尺度缩放)。
        double a = wx - ox;
        double b = wy - oy;
        // the minimum value to raytrace from is the origin
        // 2)要进行射线扫描的最小值是原点
        if (wx < origin_x)
        {
            double t = (origin_x - ox) / a;
            wx = origin_x;
            wy = oy + b * t;
        }
        if (wy < origin_y)
        {
            double t = (origin_y - oy) / b;
            wx = ox + a * t;
            wy = origin_y;
        }
        // the maximum value to raytrace to is the end of the map
        // 3)要进行射线扫描的最大值是地图的末尾
        if (wx > map_end_x)
        {
            double t = (map_end_x - ox) / a;
            wx = map_end_x - .001;
            wy = oy + b * t;
        }
        if (wy > map_end_y)
        {
            double t = (map_end_y - oy) / b;
            wx = ox + a * t;
            wy = map_end_y - .001;
        }
        // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
        // 4)现在vector已正确缩放...我们将获取其端点的地图坐标
        unsigned int x1, y1;
        // 5)check for legality just in case 检查wx, wy在map中是否有效
        if (!worldToMap(wx, wy, x1, y1)) continue;
        // 给定世界上的距离…将其转换为单元格
        unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
        MarkCell marker(costmap_, FREE_SPACE);
        // and finally... we can execute our trace to clear obstacles along that line
        // 6)最后...我们可以执行跟踪以清除该线上的障碍物
        raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
        // 7)更新射线边界 保证连线上的一点（距离传感器特定范围内）被包含进bound。
        updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
    }
}

void ObstacleLayer::activate()
{
    // if we're stopped we need to re-subscribe to topics
    for (auto & observation_subscriber : observation_subscribers_)
    {
        if (observation_subscriber != NULL)
            observation_subscriber->subscribe();
    }
    for (auto & observation_buffer : observation_buffers_)
    {
        if (observation_buffer)
            observation_buffer->resetLastUpdated();
    }
}

void ObstacleLayer::deactivate()
{
    for (auto & observation_subscriber : observation_subscribers_)
    {
        if (observation_subscriber != NULL)
            observation_subscriber->unsubscribe();
    }
}

void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
                                         double *min_x, double *min_y, double *max_x, double *max_y)
{
    double dx = wx - ox, dy = wy - oy;
    double full_distance = hypot(dx, dy);
    double scale = std::min(1.0, range / full_distance);
    double ex = ox + dx * scale, ey = oy + dy * scale;
    touch(ex, ey, min_x, min_y, max_x, max_y);
}

void ObstacleLayer::reset()
{
    deactivate();
    resetMaps();
    current_ = true;
    activate();
}

}  // namespace costmap_2d
